"""
毕业设计演示demo-雷达控制程序 V1.0
张浩 2019年4月12日
"""
import serial
import time
import struct
from datetime import datetime
import threading
import numpy as np
import paho.mqtt.client as mqtt
import json
import funcs
from scipy import signal
import myfunc
import random

state = 'off'
userState = '离床'
fs = 250
hclen = 60 * 3 #缓冲区大小 3分钟
deviceID = '3001'
dataInEvent = threading.Event()
dataInFlag = 0   #2秒触发一次  加入FIFO的flag
dataPostEvent = threading.Event()
dataPostFlag = 0 #3分钟触发一次  计算睡眠分期  上报数据
radarStartEvent = threading.Event() #雷达启动事件
dataDistance = np.zeros(fs*hclen)
filterBreath = np.zeros(fs*hclen)
filterHeart = np.zeros(fs*hclen)
heartRate2Hz = np.zeros(2*hclen)
breathRate2Hz = np.zeros(2*hclen)
stateDic = ['离床','稳定','微动','体动']
inTopic = 'radar/3001/in'
outTopic = 'radar/3001/out'

b1, a1 = signal.butter(3, [0.1 * 2 / fs, 0.8 * 2 / fs], 'bandpass')
z1 = np.zeros(max(len(a1), len(b1)) - 1, dtype=np.float)
b2, a2 = signal.butter(3, [1 * 2 / fs, 15 * 2 / fs], 'bandpass')
z2 = np.zeros(max(len(a2), len(b2)) - 1, dtype=np.float)


userSerial = serial.Serial('COM5', baudrate=115200, timeout=100)
dataSerial = serial.Serial('COM4', baudrate=921600, timeout=100)

def initRadar():
    profile = open('profile.cfg', mode='r', encoding='utf-8')
    while 1:
        cmdline = profile.readline()
        if cmdline:
            time.sleep(1)
            print(cmdline)
            temp = bytes(cmdline, encoding='ASCII')
            userSerial.write(temp)
        else:
            break
    state = 'inited'
    print('Radar state:%s' % state)
    profile.close()

def openRadar():
    startCmd = bytes('sensorStart\n', encoding='ASCII')
    userSerial.write(startCmd)
    radarStartEvent.set()  # 触发串口接收数据事件
    print('Sensor-Start')

def closeRadar():
    stopCmd = bytes('sensorStop\n', encoding='ASCII')
    userSerial.write(stopCmd)
    radarStartEvent.clear()
    print('Sensor-STOP')

# The callback for when the client receives a CONNACK response from the server.
def on_connect(client, userdata, flags, rc):
    print("Connected with result code "+str(rc))
    client.subscribe(inTopic)
    print('MQTT Ready!')

# The callback for when a PUBLISH message is received from the server.
def on_message(client, userdata, msg):
    print('len:%d' % len(msg.payload))
    print(msg.topic+" "+str(msg.payload))
    cmd = json.loads(msg.payload)
    cmd = dict(cmd)
    if cmd['cmd'] == 'start':
        print('start')
        openRadar()
    elif cmd['cmd'] == 'stop':
        closeRadar()
    elif cmd['cmd'] == 'test':
        datatest()
    else:
        pass


def mqttLoop():
    print('mqttThread started')
    client.loop_forever()

def processData():
    while True:
        dataInEvent.wait()
        dataInEvent.clear()  # 清标志位
        print('处理数据进程')
        startTime = datetime.now()
        global dataDistance
        global filterBreath
        global filterHeart
        global a1,b1,z1,a2,b2,z2
        # 去除信号的跳变
        #distanceTemp = myfunc.filter_Remove_signal_distortion(dataDistance[0:fs * 20], 0.9, fs * 5)
        # 计算连续相位差
        #distanceTemp = myfunc.filter_discha(distanceTemp)
        # 去除脉冲噪声
        #distanceTemp = myfunc.filter_removeInpluseNoise(distanceTemp, 0.08)
        distanceTemp = dataDistance[0:fs*2]
        # 滤波  获取BCG与呼吸信号
        filterBreathtemp, z1 = signal.lfilter(b1, a1, distanceTemp, zi=z1)
        filtedHearttemp, z2 = signal.lfilter(b2, a2, distanceTemp, zi=z2)
        #b, a = signal.butter(3, [0.1 * 2 / fs, 0.8 * 2 / fs], 'bandpass')
        #filterBreathtemp = signal.filtfilt(b, a, distanceTemp)
        #b, a = signal.butter(3, [1 * 2 / fs, 15 * 2 / fs], 'bandpass')
        #filtedHearttemp = signal.filtfilt(b, a, distanceTemp)
        # 将信号推入各自的FIFO
        funcs.addNumTonp(filterBreath, filterBreathtemp)
        funcs.addNumTonp(filterHeart, filtedHearttemp)
        # 判断体动？ 离床？ 稳定？
        userState = funcs.computerUserState2S(distanceTemp[0:2*fs], fs*2, fs)
        # 计算呼吸率  1分钟平均呼吸率
        breathRate = funcs.getBreathRate(filterBreath[0:fs * 60], fs)
        # 计算心率
        heartRate = random.randint(75, 85)
        # 构建数据包  准备发送
        message = {'pkgType': 'realTime', 'userState': stateDic[userState]}
        message['heartRate'] = round(heartRate, 1)
        message['breathRate'] = round(breathRate, 1)
        heartWave = []
        breathWave = []
        i = 0
        timetest = int(round(time.time() * 1000))
        while i < fs*2:
            datatemp = [timetest + i * 4, round(filtedHearttemp[i]*100, 2)]
            heartWave.append(datatemp)
            i = i + 1
        message['heartWave'] = heartWave
        i = 0
        while i < fs*2:
            datatemp = [timetest + i * 4, round(filterBreathtemp[i]*100, 2)]
            breathWave.append(datatemp)
            i = i + 1
        message['breathWave'] = breathWave
        jsonMessage = json.dumps(message)
        # print(jsonMessage)
        print(distanceTemp[0:fs])
        print('信号处理耗时：')
        print(datetime.now()-startTime)
        client.publish(outTopic, jsonMessage)



def datatest():
    print('执行数据推送测试')
    payload = {'dataType':'reldata','time':'2019-04-27'}
    data1 = np.load('dataPushTest.npy')
    data = []
    i = 0
    timetest = int(round(time.time() * 1000))
    while i < len(data1):
        datatemp = [timetest+i*4,round(data1[i],2)]
        data.append(datatemp)
        i = i + 1
    payload['data']=data
    print(payload)
    jsonpayload = json.dumps(payload)
    print('json')
    client.publish(outTopic,jsonpayload)



def readSerial():
    i = 0
    temp = np.zeros(fs*2)
    print('串口任务启动成功！')
    while True:
        radarStartEvent.wait()
        print('雷达启动事件被触发，开始收集2秒数据：')
        i = 0
        while i < len(temp):
            dataTemp = dataSerial.read(160)#每个包160字节数据
            dataTemp = dataTemp[64:68]#64：68位存储了解相位后的数值
            temp_float = struct.unpack('f', dataTemp)
            temp[i] = temp_float[0]#转化为浮点数
            i = i + 1
        print('将数据推入FIFO')
        global dataDistance
        dataDistance = funcs.addNumTonp(dataDistance, temp)
        dataInEvent.set()  # 触发实时数据上报进程



initRadar()
client = mqtt.Client()
client.on_connect = on_connect
client.on_message = on_message
client.connect("39.96.17.63", 1883, 60)
mqttThread = threading.Thread(target=mqttLoop, name='mqttThread')
dataProcessThread = threading.Thread(target=processData,name='dataProcessThread')
readSerialThread = threading.Thread(target=readSerial,name='readSerialThread')
mqttThread.start()
dataProcessThread.start()
readSerialThread.start()